#include "IonoCorr.h"

double IonoCorr::get_ionoCorr(BLH recPos, Azel satAzel, SOW t) {
    double Pi = this->mCons->get_Pi();
    double CLIGHT = this->mCons->get_SpeedOfLight();

    double tt, f, psi, phi, lam, amp, per, x;

    if (recPos.H < -1E3 || satAzel.ele <= 0) return 0.0;

    /* earth centered angle (semi-circle) */
    psi = 0.0137 / (satAzel.ele / Pi + 0.11) - 0.022;

    /* subionospheric latitude/longitude (semi-circle) */
    phi = recPos.B / Pi + psi * cos(satAzel.azi);
    if (phi > 0.416) phi = 0.416;
    else if (phi < -0.416) phi = -0.416;
    lam = recPos.L / Pi + psi * sin(satAzel.azi) / cos(phi * Pi);

    /* geomagnetic latitude (semi-circle) */
    phi += 0.064 * cos((lam - 1.617) * Pi);

    /* local time (s) */
    tt = 43200.0 * lam + t.sec + t.sesec;
    tt -= floor(tt / 86400.0) * 86400.0; /* 0<=tt<86400 */

    /* slant factor */
    f = 1.0 + 16.0 * pow(0.53 - satAzel.ele / Pi, 3.0);

    /* ionospheric delay */
    double data[8] = { 0.0 };
    for (int i = 0; i < 4; ++i) {
        data[i]   = this->mHead->ionAlpha[i];
        data[i+ 4]= this->mHead->ionBeta[i];
    }
    amp = data[0] + phi * (data[1] + phi * (data[2] + phi * data[3]));
    per = data[4] + phi * (data[5] + phi * (data[6] + phi * data[7]));
    amp = amp < 0.0 ? 0.0 : amp;
    per = per < 72000.0 ? 72000.0 : per;
    x = 2.0 * Pi * (tt - 50400.0) / per;
    return CLIGHT * f * (fabs(x) < 1.57 ? 5E-9 + amp * (1.0 + x * x * (-0.5 + x * x / 24.0)) : 5E-9);
}
